#include <opencv2/opencv.hpp>
#include <math.h>

using namespace std;

class ROIExtracter
{
private:
    int *finger_roots;

public:
    ROIExtracter(int finger_roots[]);
    ~ROIExtracter();
    cv::Point **extract();
    void extract(int Sx[4], int Sy[4]);
};

ROIExtracter::ROIExtracter(int finger_roots[])
{
    this->finger_roots = finger_roots;
}

ROIExtracter::~ROIExtracter()
{
}

cv::Point **ROIExtracter::extract()
{
    cv::Point **result = new cv::Point *[1];
    result[0] = new cv::Point[4];
    cv::Mat A = cv::Mat::zeros(cv::Size2d(2, 2), CV_32F);
    cv::Mat B = cv::Mat::zeros(cv::Size2d(4, 2), CV_32F);
    cv::Mat C = cv::Mat::zeros(cv::Size2d(4, 2), CV_32F);
    cv::Mat result_mat;
    int a = this->finger_roots[0];
    int b = this->finger_roots[1];
    int c = this->finger_roots[4];
    int d = this->finger_roots[5];
    float m = sqrt((a - c) * (a - c) + (b - d) * (b - d));
    const int roi_size = (float)sqrt(m * m + 400) / 1.20;
    const int bias = roi_size * 32 / 167 * 1.3;

    A.at<float>(0, 0) = (a - c) / m;
    A.at<float>(0, 1) = (b - d) / m;
    A.at<float>(1, 0) = (d - b) / m;
    A.at<float>(1, 1) = (a - c) / m;

    B.at<float>(0, 0) = roi_size / 2.0;
    B.at<float>(0, 1) = roi_size / 2.0;
    B.at<float>(0, 2) = -roi_size / 2.0;
    B.at<float>(0, 3) = -roi_size / 2.0;
    B.at<float>(1, 0) = 0 + bias;
    B.at<float>(1, 1) = roi_size + bias;
    B.at<float>(1, 2) = roi_size + bias;
    B.at<float>(1, 3) = 0 + bias;

    C.at<float>(0, 0) = (a + c) / 2;
    C.at<float>(0, 1) = (a + c) / 2;
    C.at<float>(0, 2) = (a + c) / 2;
    C.at<float>(0, 3) = (a + c) / 2;
    C.at<float>(1, 0) = (b + d) / 2;
    C.at<float>(1, 1) = (b + d) / 2;
    C.at<float>(1, 2) = (b + d) / 2;
    C.at<float>(1, 3) = (b + d) / 2;

    result_mat = A.inv() * B + C;

    result[0][0] = cv::Point(result_mat.at<float>(0, 0), result_mat.at<float>(1, 0));
    result[0][1] = cv::Point(result_mat.at<float>(0, 1), result_mat.at<float>(1, 1));
    result[0][2] = cv::Point(result_mat.at<float>(0, 2), result_mat.at<float>(1, 2));
    result[0][3] = cv::Point(result_mat.at<float>(0, 3), result_mat.at<float>(1, 3));

    return result;
}

void ROIExtracter::extract(int Sx[4], int Sy[4])
{
    //todo: result is abnormal
    //x: index finger coordinates
    //y: little finger coordinates
    int x0 = finger_roots[1], y0 = finger_roots[0], x2 = finger_roots[5], y2 = finger_roots[4];
    int m, c, cy;
    int maxValue = (x0 - x2) * (x0 - x2) + (y0 - y2) * (y0 - y2) + 400;
    float b, a, d, T;
    int xa, ya, xb, yb, xc, yc, xd, yd;
    float x3, x4, y3, y4;
    float xaf, yaf, xbf, ybf, xcf, ycf, xdf, ydf;
    float xcenter, ycenter;
    float distance = (float)sqrt((float)maxValue) / 1.20;
    float Distance2 = (float)distance * distance;
    float ROIDistFromKey = distance * 32 / 167 * 1.3;

    if (y0 != y2)
    {
        m = -(x0 - x2) / (y0 - y2);
        c = y2 - m * x2;
        cy = (c - y2);
        a = (1 + m * m);
        b = (2 * m * cy - 2 * x2);
        d = x2 * x2 + cy * cy - ROIDistFromKey * ROIDistFromKey;
        T = (float)sqrt(b * b - 4 * a * d);
        x3 = ((-b + T) / (2 * a));
        x4 = ((-b - T) / (2 * a));
        y3 = (m * x3 + c);
        y4 = (m * x4 + c);
        if (y4 > y3)
        {
            xaf = x4;
            yaf = y4;
        }
        else
        {
            xaf = x3;
            yaf = y3;
        }
        d = x2 * x2 + cy * cy - (distance + ROIDistFromKey) * (distance + ROIDistFromKey);
        T = (float)sqrt(b * b - 4 * a * d);
        x3 = ((-b + T) / (2 * a));
        x4 = ((-b - T) / (2 * a));
        y3 = (m * x3 + c);
        y4 = (m * x4 + c);
        if (y4 > y3)
        {
            xbf = x4;
            ybf = y4;
        }
        else
        {
            xbf = x3;
            ybf = y3;
        }

        c = y0 - m * x0;
        cy = (c - y0);
        a = (1 + m * m);
        b = (2 * m * cy - 2 * x0);
        d = x0 * x0 + cy * cy - ROIDistFromKey * ROIDistFromKey;
        T = (float)sqrt(b * b - 4 * a * d);
        x3 = ((-b + T) / (2 * a));
        x4 = ((-b - T) / (2 * a));
        y3 = (m * x3 + c);
        y4 = (m * x4 + c);
        if (y4 > y3)
        {
            xdf = x4;
            ydf = y4;
        }
        else
        {
            xdf = x3;
            ydf = y3;
        }

        d = x0 * x0 + cy * cy - (distance + ROIDistFromKey) * (distance + ROIDistFromKey);
        T = (float)sqrt(b * b - 4 * a * d);
        x3 = ((-b + T) / (2 * a));
        x4 = ((-b - T) / (2 * a));
        y3 = (m * x3 + c);
        y4 = (m * x4 + c);
        if (y4 > y3)
        {
            xcf = x4;
            ycf = y4;
        }
        else
        {
            xcf = x3;
            ycf = y3;
        }
    }
    else
    {
        distance = (float)sqrt(Distance2);
        xaf = x2;
        xbf = x2;
        xcf = x0;
        xdf = x0;
        yaf = y2 + ROIDistFromKey;
        ybf = y2 + ROIDistFromKey + distance;
        ycf = y0 + ROIDistFromKey + distance;
        ydf = y0 + ROIDistFromKey;
    }
    xa = (int)(xaf + 0.5);
    xb = (int)(xbf + 0.5);
    xc = (int)(xcf + 0.5);
    xd = (int)(xdf + 0.5);
    ya = (int)(yaf + 0.5);
    yb = (int)(ybf + 0.5);
    yc = (int)(ycf + 0.5);
    yd = (int)(ydf + 0.5);

    if (xa != xd)
    {
        xcenter = (xaf + xdf) / 2;
        ycenter = (yaf + ydf) / 2;
        m = (yaf - ydf) / (xaf - xdf);
        c = ycenter - m * xcenter;
        cy = c - ycenter;
        a = (1 + m * m);
        b = 2 * m * cy - 2 * xcenter;
        d = xcenter * xcenter + cy * cy - Distance2 / 4;
        T = (float)sqrt(b * b - 4 * a * d);
        x3 = ((-b + T) / (2 * a));
        x4 = ((-b - T) / (2 * a));
        y3 = (m * x3 + c);
        y4 = (m * x4 + c);
        xd = (int)(x4 + 0.5);
        yd = (int)(y4 + 0.5);
        xa = (int)(x3 + 0.5);
        ya = (int)(y3 + 0.5);
        xcenter = (xbf + xcf) / 2;
        ycenter = (ybf + ycf) / 2;
        c = ycenter - m * xcenter;
        cy = (c - ycenter);
        a = (1 + m * m);
        b = (2 * m * cy - 2 * xcenter);
        d = xcenter * xcenter + cy * cy - Distance2 / 4;
        T = (float)sqrt(b * b - 4 * a * d);
        x3 = ((-b + T) / (2 * a));
        x4 = ((-b - T) / (2 * a));
        y3 = (m * x3 + c);
        y4 = (m * x4 + c);
        xc = (int)(x4 + 0.5);
        yc = (int)(y4 + 0.5);
        xb = (int)(x3 + 0.5);
        yb = (int)(y3 + 0.5);
    }
    else
    {
        xa = (int)xaf;
        xb = (int)xbf;
        xc = (int)xbf;
        xd = (int)xaf;
        ya = (int)(yaf + ydf) / 2 + (int)(distance / 2);
        yb = (int)(ybf + ycf) / 2 + (int)(distance / 2);
        yc = (int)(ybf + ycf) / 2 - (int)(distance / 2);
        yd = (int)(yaf + ydf) / 2 - (int)(distance / 2);
    }

    Sx[0] = xa;
    Sx[1] = xb;
    Sx[2] = xc;
    Sx[3] = xd;
    Sy[0] = ya;
    Sy[1] = yb;
    Sy[2] = yc;
    Sy[3] = yd;
}